/*
 * er9x_mixer.c
 *
 * Created: 22/08/2011 16:19:22
 *  Author: guy
 */ 
#include <stdint.h>
#include "pipo.h"
#include "pwm.h"
#include "mixer.h"
#include "eepromData.h"

mixData_t *mixaddress( uint8_t idx )
{
	return &pipoData.mixData[idx] ;
}

bool tick10ms =false; //####TODO #task #82

bool keyState(EnumKeys enuk) //####TODO #task #80
{
/*
  if(enuk < (int)DIM(keys))  return keys[enuk].state() ? 1 : 0;
  switch(enuk){
    case SW_ElevDR : return PINE & (1<<INP_E_ElevDR);
    
    //case SW_AileDR : return PINE & (1<<INP_E_AileDR);
#if (!(defined(JETI) || defined(FRSKY) || defined(ARDUPILOT)))
    case SW_AileDR : return PINE & (1<<INP_E_AileDR);
#else
    case SW_AileDR : return PINC & (1<<INP_C_AileDR); //shad974: rerouted inputs to free up UART0
#endif


    case SW_RuddDR : return PING & (1<<INP_G_RuddDR);
      //     INP_G_ID1 INP_E_ID2
      // id0    0        1
      // id1    1        1
      // id2    1        0
    case SW_ID0    : return !(PING & (1<<INP_G_ID1));
    case SW_ID1    : return (PING & (1<<INP_G_ID1))&& (PINE & (1<<INP_E_ID2));
    case SW_ID2    : return !(PINE & (1<<INP_E_ID2));
    case SW_Gear   : return PINE & (1<<INP_E_Gear);
    //case SW_ThrCt  : return PINE & (1<<INP_E_ThrCt);

#if (!(defined(JETI) || defined(FRSKY) || defined(ARDUPILOT)))
     case SW_ThrCt  : return PINE & (1<<INP_E_ThrCt);
#else
    case SW_ThrCt  : return PINC & (1<<INP_C_ThrCt); //shad974: rerouted inputs to free up UART0
#endif

    case SW_Trainer: return PINE & (1<<INP_E_Trainer);
    default:;
  }*/
  return 0;
}

inline int16_t getValue(uint8_t i) //####TODO sort out what this is doing??? task #81
{
  //gh if(i<PPM_BASE) return calibratedStick[i];//-512..512
    if(i<PPM_BASE) return (pwmInRaw(i+1)-pwmInBias(i+1))/2; //gh
/*
  else if(i<PPM_BASE+4) return (g_ppmIns[i-PPM_BASE] - g_eeGeneral.trainer.calib[i-PPM_BASE])*2;
  else if(i<CHOUT_BASE) return g_ppmIns[i-PPM_BASE]*2;
  else if(i<CHOUT_BASE+NUM_CHNOUT) return ex_chans[i-CHOUT_BASE];
  else return 0;
*/
	else return 0;
}


bool getSwitch(int8_t swtch, bool nc, uint8_t level)
{
  if(level>5) return false; //prevent recursive loop going too deep

  switch(swtch){
    case  0:            return  nc;
    case  MAX_DRSWITCH: return  true;
    case -MAX_DRSWITCH: return  false;
  }

  uint8_t dir = swtch>0;
  if(abs(swtch)<(MAX_DRSWITCH-NUM_CSW)) {
    if(!dir) return ! keyState((EnumKeys)(SW_BASE-swtch-1));
    return            keyState((EnumKeys)(SW_BASE+swtch-1));
  }

  //custom switch, Issue 78
  //use putsChnRaw
  //input -> 1..4 -> sticks,  5..8 pots
  //MAX,FULL - disregard
  //ppm
  cSwData_t &cs = pipoData.customSw[abs(swtch)-(MAX_DRSWITCH-NUM_CSW)];
  if(!cs.func) return false;


  int8_t a = cs.v1;
  int8_t b = cs.v2;
  int16_t x = 0;
  int16_t y = 0;

  // init values only if needed
  uint8_t s = CS_STATE(cs.func);

  if(s == CS_VOFS)
  {
      x = getValue(cs.v1-1);
      y = calc100toRESX(cs.v2);
  }
  else if(s == CS_VCOMP)
  {
      x = getValue(cs.v1-1);
      y = getValue(cs.v2-1);
  }

  switch (cs.func) {
  case (CS_VPOS):
      return swtch>0 ? (x>y) : !(x>y);
      break;
  case (CS_VNEG):
      return swtch>0 ? (x<y) : !(x<y);
      break;
  case (CS_APOS):
  {
      bool res = (abs(x)>y) ;
      return swtch>0 ? res : !res ;
  }
//      return swtch>0 ? (abs(x)>y) : !(abs(x)>y);
      break;
  case (CS_ANEG):
  {
      bool res = (abs(x)<y) ;
      return swtch>0 ? res : !res ;
  }
//      return swtch>0 ? (abs(x)<y) : !(abs(x)<y);
      break;

//  case (CS_AND):
//      return (getSwitch(a,0,level+1) && getSwitch(b,0,level+1));
//      break;
//  case (CS_OR):
//      return (getSwitch(a,0,level+1) || getSwitch(b,0,level+1));
//      break;
//  case (CS_XOR):
//      return (getSwitch(a,0,level+1) ^ getSwitch(b,0,level+1));
//      break;
  case (CS_AND):
  case (CS_OR):
  case (CS_XOR):
  {
    bool res1 = getSwitch(a,0,level+1) ;
    bool res2 = getSwitch(b,0,level+1) ;
    if ( cs.func == CS_AND )
    {
      return res1 && res2 ;
    }
    else if ( cs.func == CS_OR )
    {
      return res1 || res2 ;
    }
    else  // CS_XOR
    {
      return res1 ^ res2 ;
    }
  }
  break;

  case (CS_EQUAL):
      return (x==y);
      break;
  case (CS_NEQUAL):
      return (x!=y);
      break;
  case (CS_GREATER):
      return (x>y);
      break;
  case (CS_LESS):
      return (x<y);
      break;
  case (CS_EGREATER):
      return (x>=y);
      break;
  case (CS_ELESS):
      return (x<=y);
      break;
  default:
      return false;
      break;
  }

}


uint16_t isqrt32(uint32_t n)
{
    uint16_t c = 0x8000;
    uint16_t g = 0x8000;

    for(;;) {
        if((uint32_t)g*g > n)
            g ^= c;
        c >>= 1;
        if(c == 0)
            return g;
        g |= c;
    }
}

int16_t intpol(int16_t x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
{
#define D9 (RESX * 2 / 8)
#define D5 (RESX * 2 / 4)
    bool    cv9 = idx >= MAX_CURVE5;
    int8_t *crv = cv9 ? pipoData.curves9[idx-MAX_CURVE5] : pipoData.curves5[idx];
    int16_t erg;

    x+=RESXu;
    if(x < 0) {
        erg = (int16_t)crv[0] * (RESX/4);
    } else if(x >= (RESX*2)) {
        erg = (int16_t)crv[(cv9 ? 8 : 4)] * (RESX/4);
    } else {
        int16_t a,dx;
        if(cv9){
            a   = (uint16_t)x / D9;
            dx  =((uint16_t)x % D9) * 2;
        } else {
            a   = (uint16_t)x / D5;
            dx  = (uint16_t)x % D5;
        }
        erg  = (int16_t)crv[a]*((D5-dx)/2) + (int16_t)crv[a+1]*(dx/2);
    }
    return erg / 25; // 100*D5/RESX;
}

uint16_t expou(uint16_t x, uint16_t k)
{
    // k*x*x*x + (1-k)*x
    return ((unsigned long)x*x*x/0x10000*k/(RESXul*RESXul/0x10000) + (RESKul-k)*x+RESKul/2)/RESKul;
}

int16_t expo(int16_t x, int16_t k)
{
    if(k == 0) return x;
    int16_t   y;
    bool    neg =  x < 0;
    if(neg)   x = -x;
    if(k<0){
        y = RESXu-expou(RESXu-x,-k);
    }else{
        y = expou(x,k);
    }
    return neg? -y:y;
}

// static variables used in perOut - moved here so they don't interfere with the stack
// It's also easier to initialize them here.
uint16_t pulses2MHz[70] = {0};
int16_t  anas [NUM_XCHNRAW] = {0};
int32_t  chans[NUM_CHNOUT] = {0};
uint8_t inacPrescale ;
uint16_t inacCounter = 0;
uint16_t inacSum = 0;
uint8_t  bpanaCenter = 0;
int16_t  sDelay[MAX_MIXERS] = {0};
int32_t  act   [MAX_MIXERS] = {0};
uint8_t  swOn  [MAX_MIXERS] = {0};

void perOut(int16_t *chanOut, uint8_t att)
{
    int16_t  trimA[4];
//gh    uint8_t  anaCenter = 0;
    uint16_t d = 0;

    /*if(tick10ms) {
        if(s_noHi) s_noHi--;
        if( (pipoData.inactivityTimer + 10) && (g_vbat100mV>49)) {
					  if (++inacPrescale > 15 )
						{
              inacCounter++;
							inacPrescale = 0 ;
						}
            uint16_t tsum = 0;
            for(uint8_t i=0;i<4;i++) tsum += anas[i];
            if(abs(int16_t(tsum-inacSum))>INACTIVITY_THRESHOLD){
                inacSum = tsum;
                inacCounter=0;
            }
            if(inacCounter>((uint16_t)(pipoData.inactivityTimer+10)*100*60/16))
                if((inacCounter&0x3)==1) beepWarn();
        }
    }*/

    //===========Swash Ring================
    if(pipoData.swashRingValue)
    {
        uint32_t v = (int32_t((pwmInRaw(PITCH_CHN)-pwmInBias(PITCH_CHN)))*((pwmInRaw(PITCH_CHN)-pwmInBias(PITCH_CHN))) +
                      int32_t((pwmInRaw(ROLL_CHN)-pwmInBias(ROLL_CHN)))*(pwmInRaw(ROLL_CHN)-pwmInBias(ROLL_CHN)));
        uint32_t q = int32_t(RESX)*pipoData.swashRingValue/100;
        q *= q;
        if(v>q)
            d = isqrt32(v);
    }
    //===========Swash Ring================

    for(uint8_t i=0;i<7;i++){        // calc Sticks

        //Normalization  [0..2048] ->   [-1024..1024]

        int16_t v = pwmInRaw(i+1) - pwmInBias(i+1);//gh anaIn(i);
/*gh
        v -= pipoData.calibMid[i];
        v  =  v * (int32_t)RESX /  (max((int16_t)100,(v>0 ?
                                                          pipoData.calibSpanPos[i] :
                                                          pipoData.calibSpanNeg[i])));
        if(v <= -RESX) v = -RESX;
        if(v >=  RESX) v =  RESX;
*/
//gh!!!!        calibratedStick[i] = v; //for show in expo
/*gh
		if(!(v/16)) anaCenter |= 1<<(CONVERT_MODE((i+1))-1);
*/


        if(i<4) { //only do this for sticks
 /*gh			//===========Trainer mode================
            if (!(att&NO_TRAINER) && pipoData.traineron) {
                TrainerMix* td = &pipoData.trainer.mix[i];
                if (td->mode && getSwitch(td->swtch, 1)) {
                    uint8_t chStud = td->srcChn;
                    int16_t vStud  = (g_ppmIns[chStud]- pipoData.trainer.calib[chStud]) / * *2 * / ;
                    vStud /= 2 ;		// Only 2, because no *2 above
                    vStud *= td->studWeight ;
                    vStud /= 31 ;
                    vStud *= 4 ;
                    switch (td->mode) {
                    case 1: v += vStud;   break; // add-mode
                    case 2: v  = vStud;   break; // subst-mode
                    }
                }
            }*/

            //===========Swash Ring================
            if(d && (i==PITCH_CHN || i==ROLL_CHN))
                v = int32_t(v)*pipoData.swashRingValue*RESX/(int32_t(d)*100);
            //===========Swash Ring================

            uint8_t expoDrOn = GET_DR_STATE(i);
            uint8_t stkDir = v>0 ? DR_RIGHT : DR_LEFT;

            /*if(IS_THROTTLE(i) && pipoData.thrExpo){
                v  = 2*expo((v+RESX)/2,pipoData.expoData[i].expo[expoDrOn][DR_EXPO][DR_RIGHT]);
                stkDir = DR_RIGHT;
            }
            else*/
                v  = expo(v,pipoData.expoData[i].expo[expoDrOn][DR_EXPO][stkDir]);

            int32_t x = (int32_t)v * (pipoData.expoData[i].expo[expoDrOn][DR_WEIGHT][stkDir]+100)/100;
            v = (int16_t)x;
/*gh
            if (IS_THROTTLE(i) && pipoData.thrExpo) v -= RESX;
*/

/*gh
            //do trim -> throttle trim if applicable
            int32_t vv = 2*RESX;
            if(IS_THROTTLE(i) && pipoData.thrTrim) vv = ((int32_t)*TrimPtr[i]+125)*(RESX-v)/(2*RESX);

            //trim
            trimA[i] = (vv==2*RESX) ? *TrimPtr[i]*2 : (int16_t)vv*2; //    if throttle trim -> trim low end
*/
        }
        anas[i] = v; //set values for mixer
    }

    //===========BEEP CENTER================
    /*anaCenter &= pipoData.beepANACenter;
    if(((bpanaCenter ^ anaCenter) & anaCenter)) beepWarn1();
    bpanaCenter = anaCenter;*/

    anas[MIX_MAX-1]  = RESX;     // MAX
    anas[MIX_FULL-1] = RESX;     // FULL
/*
    for(uint8_t i=0;i<4;i++) anas[i+PPM_BASE] = (g_ppmIns[i] - pipoData.trainer.calib[i])*2; //add ppm channels
    for(uint8_t i=4;i<NUM_PPM;i++)    anas[i+PPM_BASE]   = g_ppmIns[i]*2; //add ppm channels
*/
    for(uint8_t i=0;i<NUM_CHNOUT;i++) anas[i+CHOUT_BASE] = chans[i]; //other mixes previous outputs

    //===========Swash Ring================
    if(pipoData.swashRingValue)
    {
        uint32_t v = ((int32_t)anas[PITCH_CHN]*anas[PITCH_CHN] + (int32_t)anas[ROLL_CHN]*anas[ROLL_CHN]);
        uint32_t q = (int32_t)RESX*pipoData.swashRingValue/100;
        q *= q;
        if(v>q)
        {
            uint16_t d = isqrt32(v);
            anas[PITCH_CHN] = (int32_t)anas[PITCH_CHN]*pipoData.swashRingValue*RESX/((int32_t)d*100);
            anas[ROLL_CHN] = (int32_t)anas[ROLL_CHN]*pipoData.swashRingValue*RESX/((int32_t)d*100);
        }
    }

#define REZ_SWASH_X(x)  ((x) - (x)/8 - (x)/128 - (x)/512)   //  1024*sin(60) ~= 886
#define REZ_SWASH_Y(x)  ((x))   //  1024 => 1024

    if(pipoData.swashType)
    {
        int16_t vp = anas[PITCH_CHN]+trimA[PITCH_CHN];
        int16_t vr = anas[ROLL_CHN]+trimA[ROLL_CHN];
        int16_t vc = 0;
        if(pipoData.swashCollectiveSource)
            vc = anas[pipoData.swashCollectiveSource-1];

        if(pipoData.swashInvertELE) vp = -vp;
        if(pipoData.swashInvertAIL) vr = -vr;
        if(pipoData.swashInvertCOL) vc = -vc;

        switch (pipoData.swashType)
        {
        case (SWASH_TYPE_120):
            vp = REZ_SWASH_Y(vp);
            vr = REZ_SWASH_X(vr);
            anas[MIX_CYC1-1] = vc - vp;
            anas[MIX_CYC2-1] = vc + vp/2 + vr;
            anas[MIX_CYC3-1] = vc + vp/2 - vr;
            break;
        case (SWASH_TYPE_120X):
            vp = REZ_SWASH_X(vp);
            vr = REZ_SWASH_Y(vr);
            anas[MIX_CYC1-1] = vc - vr;
            anas[MIX_CYC2-1] = vc + vr/2 + vp;
            anas[MIX_CYC3-1] = vc + vr/2 - vp;
            break;
        case (SWASH_TYPE_140):
            vp = REZ_SWASH_Y(vp);
            vr = REZ_SWASH_Y(vr);
            anas[MIX_CYC1-1] = vc - vp;
            anas[MIX_CYC2-1] = vc + vp + vr;
            anas[MIX_CYC3-1] = vc + vp - vr;
            break;
        case (SWASH_TYPE_90):
            vp = REZ_SWASH_Y(vp);
            vr = REZ_SWASH_Y(vr);
            anas[MIX_CYC1-1] = vc - vp;
            anas[MIX_CYC2-1] = vc + vr;
            anas[MIX_CYC3-1] = vc - vr;
            break;
        default:
            break;
        }
    }

//!!!!    if(tick10ms) trace(); //trace thr 0..32  (/32)

    memset(chans,0,sizeof(chans));        // All outputs to 0

/*gh
    if(att&NO_INPUT) { //zero input for setStickCenter()
        for(uint8_t i=0;i<4;i++) {
            if(!IS_THROTTLE(i)) {
                anas[i]  = 0;
                trimA[i] = 0;
            }
        }
        for(uint8_t i=0;i<4;i++) anas[i+PPM_BASE] = 0;
    }*/

/*gh
    uint8_t mixWarning = 0;
    // Set the trim pointers back to the master set
    TrimPtr[0] = &pipoData.trim[0] ;
    TrimPtr[1] = &pipoData.trim[1] ;
    TrimPtr[2] = &pipoData.trim[2] ;
    TrimPtr[3] = &pipoData.trim[3] ;
*/

    //========== MIXER LOOP ===============

    for(uint8_t i=0;i<MAX_MIXERS;i++){
        mixData_t *md = mixaddress( i ) ;

        if((md->destCh==0) || (md->destCh>NUM_CHNOUT)) break;

        //Notice 0 = NC switch means not used -> always on line
        int16_t v  = 0;
        uint8_t swTog;

        //swOn[i]=false;
        if(!getSwitch(md->swtch,1,0)){ // switch on?  if no switch selected => on
            swTog = swOn[i];
            swOn[i] = false;
//            if(md->srcRaw==MIX_MAX) act[i] = 0;// MAX back to 0 for slow up
//            if(md->srcRaw!=MIX_FULL) continue;// if not FULL - next loop
//            v = -RESX; // switch is off  => FULL=-RESX

            if(md->srcRaw!=MIX_MAX && md->srcRaw!=MIX_FULL) continue;// if not MAX or FULL - next loop
            if(md->mltpx==MLTPX_REP) continue; // if switch is off and REPLACE then off
            v = (md->srcRaw == MIX_FULL ? -RESX : 0); // switch is off and it is either MAX=0 or FULL=-512
        }
        else {
            swTog = !swOn[i];
            swOn[i] = true;
            uint8_t k = md->srcRaw-1;
            v = anas[k]; //Switch is on. MAX=FULL=512 or value.
            if(k>=CHOUT_BASE && (k<i)) v = chans[k]; // if we've already calculated the value - take it instead // anas[i+CHOUT_BASE] = chans[i]
/*gh
            if(md->mixWarn) mixWarning |= 1<<(md->mixWarn-1); // Mix warning
            if ( md->enableFmTrim )
            {
                if ( md->srcRaw <= 4 )
                {
                    TrimPtr[md->srcRaw-1] = &md->sOffset ;		// Use the value stored here for the trim
                }
            }
*/
        }

/*gh
        //========== INPUT OFFSET ===============
        if ( md->enableFmTrim == 0 )
        {
            if(md->sOffset) v += calc100toRESX(md->sOffset);
        }
*/

        //========== DELAY and PAUSE ===============
        if (md->speedUp || md->speedDown || md->delayUp || md->delayDown)  // there are delay values
        {
#define DEL_MULT 256

            //if(init) {
            //act[i]=(int32_t)v*DEL_MULT;
            //swTog = false;
            //}
            int16_t diff = v-act[i]/DEL_MULT;

            if(swTog) {
                //need to know which "v" will give "anas".
                //curves(v)*weight/100 -> anas
                // v * weight / 100 = anas => anas*100/weight = v
                if(md->mltpx==MLTPX_REP)
                {
                    act[i] = (int32_t)anas[md->destCh-1+CHOUT_BASE]*DEL_MULT;
                    act[i] *=100;
                    if(md->weight) act[i] /= md->weight;
                }
                diff = v-act[i]/DEL_MULT;
                if(diff) sDelay[i] = (diff<0 ? md->delayUp :  md->delayDown) * 100;
            }

            if(sDelay[i]){ // perform delay
                if(tick10ms) sDelay[i]--;
                v = act[i]/DEL_MULT;
                diff = 0;
            }

            if(diff && (md->speedUp || md->speedDown)){
                //rate = steps/sec => 32*1024/100*md->speedUp/Down
                //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown);
                //-100..100 => 32768 ->  100*83886/256 = 32768,   For MAX we divide by 2 sincde it's asymmetrical
                if(tick10ms) {
                    int32_t rate = (int32_t)DEL_MULT*2048*100;
                    if(md->weight) rate /= abs(md->weight);
                    act[i] = (diff>0) ? ((md->speedUp>0)   ? act[i]+(rate)/((int16_t)100*md->speedUp)   :  (int32_t)v*DEL_MULT) :
                                        ((md->speedDown>0) ? act[i]-(rate)/((int16_t)100*md->speedDown) :  (int32_t)v*DEL_MULT) ;
                }

                if(((diff>0) && (v<(act[i]/DEL_MULT))) || ((diff<0) && (v>(act[i]/DEL_MULT)))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow
                v = act[i]/DEL_MULT;
            }
        }


        //========== CURVES ===============
        switch(md->curve){
        case 0:
            break;
        case 1:
            if(md->srcRaw == MIX_FULL) //FUL
            {
                if( v<0 ) v=-RESX;   //x|x>0
                else      v=-RESX+2*v;
            }else{
                if( v<0 ) v=0;   //x|x>0
            }
            break;
        case 2:
            if(md->srcRaw == MIX_FULL) //FUL
            {
                if( v>0 ) v=RESX;   //x|x<0
                else      v=RESX+2*v;
            }else{
                if( v>0 ) v=0;   //x|x<0
            }
            break;
        case 3:       // x|abs(x)
            v = abs(v);
            break;
        case 4:       //f|f>0
            v = v>0 ? RESX : 0;
            break;
        case 5:       //f|f<0
            v = v<0 ? -RESX : 0;
            break;
        case 6:       //f|abs(f)
            v = v>0 ? RESX : -RESX;
            break;
        default: //c1..c16
            v = intpol(v, md->curve - 7);
        }

        //========== TRIM ===============
        if((md->carryTrim==0) && (md->srcRaw>0) && (md->srcRaw<=4)) v += trimA[md->srcRaw-1];  //  0 = Trim ON  =  Default

        //========== MULTIPLEX ===============
        int32_t dv = (int32_t)v*md->weight;
        switch(md->mltpx){
        case MLTPX_REP:
            chans[md->destCh-1] = dv;
            break;
        case MLTPX_MUL:
            chans[md->destCh-1] *= dv/100l;
            chans[md->destCh-1] /= RESXl;
            break;
        default:  // MLTPX_ADD
            chans[md->destCh-1] += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100);
            break;
        }
    }

/*gh
    //========== MIXER WARNING ===============
    //1= 00,08
    //2= 24,32,40
    //3= 56,64,72,80
    {
        uint16_t tmr10ms ;
        tmr10ms = get_tmr10ms() ;
        if(mixWarning & 1) if(((tmr10ms&0xFF)==  0)) beepWarn1();
        if(mixWarning & 2) if(((tmr10ms&0xFF)== 64) || ((tmr10ms&0xFF)== 72)) beepWarn1();
        if(mixWarning & 4) if(((tmr10ms&0xFF)==128) || ((tmr10ms&0xFF)==136) || ((tmr10ms&0xFF)==144)) beepWarn1();
    }*/

    //========== LIMITS ===============
    for(uint8_t i=0;i<NUM_CHNOUT;i++){
        // chans[i] holds data from mixer.   chans[i] = v*weight => 1024*100
        // later we multiply by the limit (up to 100) and then we need to normalize
        // at the end chans[i] = chans[i]/100 =>  -1024..1024
        // interpolate value with min/max so we get smooth motion from center to stop
        // this limits based on v original values and min=-1024, max=1024  RESX=1024

        int32_t q = chans[i];// + (int32_t)pipoData.limitData[i].offset*100; // offset before limit

        chans[i] /= 100; // chans back to -1024..1024
//!!!!        ex_chans[i] = chans[i]; //for getswitch

        int16_t ofs = pipoData.limitData[i].offset;
        int16_t lim_p = 10*(pipoData.limitData[i].max+100);
        int16_t lim_n = 10*(pipoData.limitData[i].min-100); //multiply by 10 to get same range as ofs (-1000..1000)
        if(ofs>lim_p) ofs = lim_p;
        if(ofs<lim_n) ofs = lim_n;

        if(q) q = (q>0) ?
                    q*((int32_t)lim_p-ofs)/100000 :
                    -q*((int32_t)lim_n-ofs)/100000 ; //div by 100000 -> output = -1024..1024

        q += calc1000toRESX(ofs);
        lim_p = calc1000toRESX(lim_p);
        lim_n = calc1000toRESX(lim_n);
        if(q>lim_p) q = lim_p;
        if(q<lim_n) q = lim_n;
        if(pipoData.limitData[i].revert) q=-q;// finally do the reverse.

/*gh
        if(pipoData.safetySw[i].swtch)  //if safety sw available for channel check and replace val if needed
            if(getSwitch(pipoData.safetySw[i].swtch,0)) q = calc100toRESX(pipoData.safetySw[i].val);
*/

        cli();
        chanOut[i] = q; //copy consistent word to int-level
        sei();
    }
}